#include "adc_handle.h"

#define TIMES 0

u8 ring_state = 0;
u8 fork_state = 0;
u8 stop_state = 0;
u8 ramp_state = 0;

u16 Ramp_Delay = 0; //坡道延时通过
int ramp_over = 0;  //在判断坡道之后才有可能为三叉

//起跑和停止
u16 Run_Delay = 0; //目的是防止启动后判断三岔后停车
char Run_Flag = 0;
char Fork_Stop_Flag = 0;

//电磁环岛内部使用
char left_ring_mode = 0;
char right_ring_mode = 0;
u16 Big_Ring_Delay_LiChuang = 0; //未使用（现在）
u16 Ring_Delay = 0;
int Ring_Size = 0;
char Ramp_flag = 0;

int left_time = 0;
int right_time = 0;

enum ring_mode
{
    Bigring = 1,
    Midring,
    Smallring,
}; //环岛大小

void stop_handle(void)
{
    int static i = 0;
    int joy = 0;
    switch (stop_state)
    {
    case stop_nostop:
    {
        if (Horizontal_R < 5 && Horizontal_L < 5 && gpio_get(G0) != 0)
        {
            stop_state = stop_outstop;
        }
        else
        {
            Target_Speed = Gear[0];
            /*速度决策*/
            if (flag.fork == 0 && flag.ramp == 0 && flag.ring == 0)
            {
                joy = Speed_exDecision(Pixle);
                Speed_Chose(joy);
            }
            if (flag.ramp == 1)
            {
                if (Ramp_flag == 0)
                {
                    Target_Speed = 400;
                    Ramp_Delay = 100;
                    Ramp_flag = 1;
                }
            }
            if (Ramp_Delay == 0)
            {
                flag.ramp = 0;
                Ramp_flag = 0;
            }
        }
        if (i == 1)
        {
            pwm_enable(TIM_5);
            mypwm_init(TIM_5, TIM_5_CH3_A02, 0, 6400, 0); //左
            mypwm_init(TIM_5, TIM_5_CH1_A00, 0, 6400, 0); //右
            Target_Speed = Gear[0];
            if (flag.fork == 0 && flag.ramp == 0 && flag.ring == 0)
            {
                joy = Speed_exDecision(Pixle);
                Speed_Chose(joy);
            }
            i = 0;
        }
    };
    break;

    case stop_outstop:
    {
        Target_Speed = 0;
        pwm_disable(TIM_5);
        flag.fork = 0;
        flag.ramp = 0;
        flag.ring = 0; //将参数清零
        if (Horizontal_L > 5 && Horizontal_R > 5 && Fork_Stop_Flag == 0)
        {
            stop_state = stop_nostop;
            i = 1;
        }
    };
    break;

    default:
        break;
    }
}

/************************
     @新增了大中小环，需要在代码中修改大中小的顺序，以及给的偏置
*************************/
void ring_handle(void)
{
    switch (ring_state)
    {
    case ring_noring:
    {
        /*
        	@明显可以看出 Chevron_outR/Chevron_outL 中的某一个为判断到了环岛的必要条件
        	@主要区别和十字还有车身歪了的时候导致的误判
        	@
        */

            L) && (Chevron_outL > 15))) //右侧环岛，此时需要保证右侧水平的值大于一个值，保证不是因为车歪了导致达到较大值
        {
            ring_state = ring_rightring;
            right_ring_mode = 1; //此标志位是为了在real_out阶段中使用，判断左侧出环还是右侧出环

            flag_buzzer = 1;
            flag.ring = 1;
            flag.fork = 0;
            flag.ramp = 0; //当在进入环岛时将其他所有的标志位记为0

            //------------------此处修改三岔大小
            Ring_Size = Midring;
#if TIMES //多环岛？
            if (right_time == 0)
            {
                Ring_Size = Midring;
            }
            else if (right_time == 1)
            {
                Ring_Size = Midring;
            }

#endif

        } //-------第一声

        //if (Chevron_outL >= 90 && Horizontal_L > 20 && Chevron_inL > 15 && Chevron_inR >10&&((30>Chevron_outR)&& (Chevron_outR>15)))//力创条件
        else if (Chevron_outL >= 90 && Horizontal_L > 20 && Chevron_inL > 15 && Chevron_inR > 10 && zebra_flag == 0)//斑马线标志位0菜鸟判断
        {
            ring_state = ring_leftring;
            left_ring_mode = 1; //此标志位是为了在real_out阶段中使用，判断左侧出环还是右侧出环

            flag_buzzer = 1;
            flag.ring = 1;
            flag.fork = 0;
            flag.ramp = 0; //当在进入环岛时将其他所有的标志位记为0

            //------------------此处修改三岔大小
            Ring_Size = Midring;
#if TIMES //多环岛？
            if (left_time == 0)
            {
                Ring_Size = Midring;
            }
            else if (left_time == 1)
            {
                Ring_Size = Midring;
            }
#endif
        }
    };
    break; //-------第一声

    case ring_rightring:
    {

        Turn_B = 0.8;
        Turn_A = 0.2;
        if (Compensate_flag == 0)
        {
            Ringin_Size_Chose(Ring_Size);
        }

        if (Ring_Delay == 0) //根据引导长度修改“Ring_Delay”的值 需要实测时间长
        {
            ring_state = ring_prein;
            flag_buzzer = 1;
#if TIMES //多环岛？
            right_time += 1;
#endif
        }
    };
    break; //-------第二声

    case ring_leftring:
    {
        Turn_B = 0.8;
        Turn_A = 0.2;
        if (Compensate_flag == 0)
        {
            Ringin_Size_Chose(Ring_Size);
        }

        if (Ring_Delay == 0) //根据引导长度修改“Ring_Delay”的值
        {
            ring_state = ring_prein;
            flag_buzzer = 1;
#if TIMES //多环岛？
            left_time += 1;
#endif
        }
    };
    break; //-------第二声

    case ring_prein:
    {
        Compensate_flag = 0;
        if (Horizontal_R + Horizontal_R < 120)
        {
            ring_state = ring_ringin;
            flag_buzzer = 1;
            Ring_Delay = 50;
        }
    };
    break; //-------第三声

    case ring_ringin:
    {
        Target_Speed = Gear[0];
        Turn_A = 0.8;
        Turn_B = 0.2;
        if (Chevron_outL > 90 || Chevron_outR > 90 || Ring_Delay == 0)
        {
            ring_state = ring_out;
            flag_buzzer = 1;
            Ring_Delay = 50;
        }
    };
    break; //-------第四声

    case ring_out:
    {
        Target_Speed = Gear[0];
        Turn_A = 0.8;
        Turn_B = 0.2;
        if ((GFP_abs(Horizontal_R - Horizontal_L) < 10 &&
             (Horizontal_R < 90 || Horizontal_L < 90)) ||
            Ring_Delay == 0)
        {
            ring_state = ring_realout;
            flag_buzzer = 1;
            Ring_Delay = 50;
        }
    }
    break;

    case ring_realout:
    {
        Ringout_Size_Chose(Ring_Size);

        if (Horizontal_L + Horizontal_R < 100 || Ring_Delay == 0)
        {
            Turn_A = 1;
            flag_buzzer = 1;
            ring_state = ring_noring;
            Compensate_flag = 0;

            flag.ring = 0; //将环岛标志位置为0
            left_ring_mode = 0;
            right_ring_mode = 0;
        }
    };
    break;

    default:
        break;
    }
}

/*
    大中小环改变控制时间和引导的大小
*/
void Ringin_Size_Chose(int Size)
{
    Compensate_flag = 1;

    switch (Size)
    {
    case Bigring:
    {
        if (right_ring_mode == 1)
        {
            Ring_Delay = 130;
            ADC_Compensate = -30; //左侧要补正，右侧为负
        }
        else if (left_ring_mode == 1)
        {
            Ring_Delay = 130;
            ADC_Compensate = 30; //左侧要补正，右侧为负
        }
    }
    break;

    case Midring:
    {
        if (right_ring_mode == 1)
        {
            Ring_Delay = 100;
            ADC_Compensate = -30; //左侧要补正，右侧为负
        }
        else if (left_ring_mode == 1)
        {
            Ring_Delay = 200;
            ADC_Compensate = 30; //左侧要补正，右侧为负
        }
    }
    break;

    case Smallring:
    {
        if (right_ring_mode == 1)
        {
            Ring_Delay = 80;
            ADC_Compensate = -30; //左侧要补正，右侧为负
        }
        else if (left_ring_mode == 1)
        {
            Ring_Delay = 80;
            ADC_Compensate = 30; //左侧要补正，右侧为负
        }
    }
    break;

    default:
        break;
    }
}

void Ringout_Size_Chose(int Size)
{
    Compensate_flag = 1;
    switch (Size)
    {
    case Bigring:
    {
        if (right_ring_mode == 1)
        {
            ADC_Compensate = 30; //左侧要负，右侧为正
        }
        else if (left_ring_mode == 1)
        {
            ADC_Compensate = -30; //左侧要负，右侧为正
        }
    }
    break;

    case Midring:
    {
        if (right_ring_mode == 1)
        {
            ADC_Compensate = 30; //左侧要负，右侧为正
        }
        else if (left_ring_mode == 1)
        {
            ADC_Compensate = -100; //左侧要负，右侧为正
        }
    }
    break;

    case Smallring:
    {
        if (right_ring_mode == 1)
        {
            ADC_Compensate = 30; //左侧要负，右侧为正
        }
        else if (left_ring_mode == 1)
        {
            ADC_Compensate = -30; //左侧要负，右侧为正
        }
    }
    break;

    default:
        break;
    }
}

void ramp_handle(void)
{
    switch (ramp_state)
    {
    case ramp_noramp:
    {
        if (flag.ramp == 1 && flag.ring == 0) //防止元素误判
        {
            ramp_state = ramp_getramp;
            Ramp_Delay = 50;
            flag_buzzer = 1;
        }
    }
    break;

    case ramp_getramp:
    {
        Target_Speed = 480; //减速
        Ramp_Delay--;
        if (Ramp_Delay == 0)
        {
            ramp_state = ramp_noramp;
            flag.ramp = 0;
            ramp_over = 1;
            flag_buzzer = 1;
        }
    }
    break;

    default:
        break;
    }
}

void Fork_Stop(void)
{
    switch (fork_state)
    {
    case fork_nofork:
    {
        if (flag.fork == 1 && Run_Flag == 1 && Fork_Delay == 0) //在环岛的条件下
        {
            if (Horizontal_L + Horizontal_R < 35)
            {
                fork_state = fork_in;
                Fork_Delay = 600;
                flag_buzzer = 1;
                uart_putchar(UART_4, 0x5B);
            }
        }
    }
    break;

    case fork_in:
    {
        if (Fork_Delay == 0)
        {
            fork_state = fork_stop;
            Fork_Delay = 100;
            flag_buzzer = 1;
        }
    }
    break;

    case fork_stop:
    {
        Target_Speed = 0;
        if (Fork_Delay == 0)
        {
            Fork_Stop_Flag = 1;
            flag_buzzer = 1;
            pwm_disable(TIM_5);
        }
    }
    }
}